/**
  ****************************RM Warrior 2023****************************
  * @file       gimbal_task.c/h
  * @brief      完成云台控制任务，由于云台使用陀螺仪解算出的角度，其范围在（-pi,pi）
  *             故而设置目标角度均为范围，存在许多对角度计算的函数。云台主要分为2种
  *             状态，陀螺仪控制状态是利用板载陀螺仪解算的姿态角进行控制，编码器控制
  *             状态是通过电机反馈的编码值控制的校准，此外还有校准状态，停止状态等。
  * @note       
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     2023/2/         pxx              ......
  *
  @verbatim
  ==============================================================================

  ==============================================================================
  @endverbatim
  ****************************RM Warrior 2023****************************111
  */

#ifndef GIMBALTASK_H
#define GIMBALTASK_H
#include "main.h"
#include "pid.h"
#include "remote_control.h"
#include "step_motor_usart.h"
#include "jy901.h"
#include "control_task.h"


//pitch 速度环 PID参数以及 PID最大输出，积分输出
#define PITCH_SPEED_PID_KP 1000.0f
#define PITCH_SPEED_PID_KI 0.0f
#define PITCH_SPEED_PID_KD 0.0f
#define PITCH_SPEED_PID_MAX_OUT 30000.0f
#define PITCH_SPEED_PID_MAX_IOUT 3000.0f

//pitch 角度环 角度由编码器 PID参数以及 PID最大输出，积分输出
#define PITCH_ENCODE_RELATIVE_PID_KP 2f
#define PITCH_ENCODE_RELATIVE_PID_KI 0.0f
#define PITCH_ENCODE_RELATIVE_PID_KD 1.2f
#define PITCH_ENCODE_RELATIVE_PID_MAX_OUT 4.0f
#define PITCH_ENCODE_RELATIVE_PID_MAX_IOUT 2.0f

//pitch 像素环      PID参数以及 PID最大输出，积分输出
#define PITCH_PIXEL_PID_KP 2.0f
#define PITCH_PIXEL_PID_KI 0.0f
#define PITCH_PIXEL_PID_KD 1.9f          //1.2
#define PITCH_PIXEL_PID_MAX_OUT 700.0f
#define PITCH_PIXEL_PID_MAX_IOUT 300.0f

//yaw 速度环 PID参数以及 PID最大输出，积分输出
#define YAW_SPEED_PID_KP 1000.0f
#define YAW_SPEED_PID_KI 0.0f
#define YAW_SPEED_PID_KD 0.0f
#define YAW_SPEED_PID_MAX_OUT  30000
#define YAW_SPEED_PID_MAX_IOUT 5000.0f

//yaw 角度环 角度由编码器 PID参数以及 PID最大输出，积分输出
#define YAW_ENCODE_RELATIVE_PID_KP 1.5f
#define YAW_ENCODE_RELATIVE_PID_KI 0.0f
#define YAW_ENCODE_RELATIVE_PID_KD 0.0f
#define YAW_ENCODE_RELATIVE_PID_MAX_OUT 4.0f
#define YAW_ENCODE_RELATIVE_PID_MAX_IOUT 2.0f

//yaw 像素环      PID参数以及 PID最大输出，积分输出
#define YAW_PIXEL_PID_KP 2.0f
#define YAW_PIXEL_PID_KI 0.0f
#define YAW_PIXEL_PID_KD 1.9f         //1,8
#define YAW_PIXEL_PID_MAX_OUT 700.0f
#define YAW_PIXEL_PID_MAX_IOUT 300.0f

//任务初始化 空闲一段时间
#define GIMBAL_TASK_INIT_TIME 201
//yaw,pitch控制通道以及状态开关通道
#define YawChannel 0
#define PitchChannel 1
//遥控器输入死区，因为遥控器存在差异，摇杆在中间，其值不一定为零
#define RC_deadband 10
//yaw，pitch角度与遥控器输入比例
#define Yaw_RC_SEN -0.000005f
#define Pitch_RC_SEN -0.000006f //0.005
//云台编码器控制时候使用的比例
#define Yaw_Encoder_Sen 0.01f 
#define Pitch_Encoder_Sen 0.01f
//云台控制周期              //500HZ                          //改动！！！！！！！！！
#define GIMBAL_CONTROL_TIME 2 

//电机码盘值最大以及中值
#define Half_ecd_range 4096
#define ecd_range 8191

// //云台初始化回中值，允许的误差,并且在误差范围内停止一段时间以及最大时间6s后解除初始化状态，
// #define GIMBAL_INIT_ANGLE_ERROR 0.05f
// #define GIMBAL_INIT_STOP_TIME 100
// #define GIMBAL_INIT_TIME 6000
// //云台初始化回中值的速度以及控制到的角度
// #define GIMBAL_INIT_PITCH_SPEED 0.003f
// #define GIMBAL_INIT_YAW_SPEED   0.006f
// #define INIT_YAW_SET 0.0f
// #define INIT_PITCH_SET 0.0f

// //判断遥控器无输入的时间以及遥控器无输入判断，设置云台yaw回中值以防陀螺仪漂移
// #define GIMBAL_MOTIONLESS_RC_DEADLINE 10
// #define GIMBAL_MOTIONLESS_TIME_MAX 3000

//电机编码值转化成弧度值
#ifndef Motor_Ecd_to_Rad
#define Motor_Ecd_to_Rad 0.000095873799242852f //      ((360/65536)*((2*pi)) /360)
#endif

#ifndef MOTOR_RAD_TO_REAL_RAD
#define MOTOR_RAD_TO_REAL_RAD 0.28125f          //9/32
#endif

#define  GIMBAL_WZ_RC_SEN        0.007   



typedef enum
{
    GIMBAL_MOTOR_RAW = 0, //电机原始值控制
    GIMBAL_MOTOR_ENCONDE, //电机编码值角度控制
} gimbal_motor_mode_e;

typedef struct
{
    fp32 kp;
    fp32 ki;
    fp32 kd;

    fp32 set;
    fp32 get;
    fp32 err;

    fp32 max_out;
    fp32 max_iout;

    fp32 Pout;
    fp32 Iout;
    fp32 Dout;

    fp32 out;
} Gimbal_PID_t;

typedef struct
{
    const motor_measure_t *gimbal_motor_measure;     //云台电机结构体
    PidTypeDef gimbal_motor_relative_angle_pid;    //相对角度pid
	PidTypeDef gimbal_motor_pixel_pid;           //相对像素pid
    PidTypeDef gimbal_motor_gyro_pid;                //yaw电机 速度电流pid
    gimbal_motor_mode_e gimbal_motor_mode;           //云台控制状态机
    gimbal_motor_mode_e last_gimbal_motor_mode;
    uint16_t offset_ecd;     //yaw电机转子中间位置
    fp32 max_relative_angle; //rad
    fp32 min_relative_angle; //rad

    fp32 relative_angle;     //rad
    fp32 relative_angle_set; //rad
    fp32 motor_gyro;         //rad/s
    fp32 motor_gyro_set;
    fp32 raw_cmd_current;
	fp32 pixel_cmd;
    fp32 current_set;        // 设定目标电流
    fp32 given_current;   // 给定的电流，用于执行
} Gimbal_Motor_t;


typedef struct
{
    const RC_ctrl_t *gimbal_rc_ctrl;    //遥控器结构体指针
    const fp32 *gimbal_INT_angle_point; //陀螺仪数据指针
    const fp32 *gimbal_INT_gyro_point;  //加速度计数据指针
    Gimbal_Motor_t gimbal_yaw_motor;    //云台yaw电机结构体
    Gimbal_Motor_t gimbal_pitch_motor;  //云台pitch电机结构体
	const pi_reveive_data  *PI_Data;    //接收数据变量

	const fp32* pixel_control;          //根据接收数据，在control中解算出的目标速度

	int * now_mode;
	int warm_num;

} Gimbal_Control_t;

extern void GIMBAL_task(void *pvParameters);
extern const Gimbal_Motor_t *get_yaw_motor_point(void);
extern const Gimbal_Motor_t *get_pitch_motor_point(void);
extern const Gimbal_Control_t *get_gimbal_control_point(void);
void gimbal_offset_init(void);//手动校正中值以及极限角度值
#endif
